
#ifndef _RRT_H_
#define _RRT_H_

#include "../Utils/Vector2D.h"
#include "../Map.h"
#include <vector>
#include <set>

struct RRTNode {
    Vector2D position;
    std::vector<RRTNode*> links;
    RRTNode *parent;
};

class RRT {
  public:
    RRT(Map *target_map, unsigned num_nodes);
    ~RRT();

    std::vector<Vector2D> planPath(Vector2D from, Vector2D to);
    void render(void); // as a debugging aid.

  private:

    std::vector<RRTNode*> all_nodes;
    Map *target_map;
    unsigned num_nodes;
    const float ship_size;

    
    // Shorten path tries to contract all of the vertices of the path 
    // as much as possible while still maining line of sight to the next 
    // vertex in the series.
    std::vector<Vector2D> shortenPath(std::vector<Vector2D> cur_path);

    std::vector<Vector2D> padPath(std::vector<Vector2D> cur_path);

    // Simplifying a path means culling out vertices which arent needed to 
    // maintain a line of sight between successive vertices.
    std::vector<Vector2D> buildSimplifiedPath(RRTNode *tail);

    RRTNode* findClosestLOSNode(Vector2D point);

    RRTNode* createNewNode(void);
    bool existsLOS(Vector2D p1, Vector2D p2, bool tight_collision_space);
  
    Vector2D generateRandomValidPoint(void);
    Vector2D generateRandomPointInMap(void);
    std::vector<Vector2D> validateLineSegment(Vector2D start, Vector2D end);
};

#endif

